#!/usr/bin/python
# -*- coding: UTF-8 -*-

import time
from MiniBotControlLib import MiniBotControlLib

class BwSensorLib:
    def __init__(self, control_lib):
        self.mclib = control_lib

    # 设置全彩LED灯传感器
    def setRGBLightSensor(self, data):
        cmd = self.mclib.SetSensorData(8, 2, data)
        if(cmd >= 1):
            print("RGB灯设置成功")
        else:
            print("RGB灯设置失败")
        return cmd

    #  读取颜色传感器
    def readColorSensor(self):
        cmd = self.mclib.GetSensorData(28, 1)
        if(cmd != -1 and cmd != 0):
            print("检测到颜色编号为：" + str(cmd))
        return cmd

    #  读取超声波传感器
    def readUltrasonicSensor(self):
        cmd = self.mclib.GetSensorData(31, 1)
        cmd = (331.5 + 0.6 * 25) * (cmd * 0.000001) / 2 * 100  # 由M 转CM *100
        print("超声波返回数据：%.1fcm" % cmd)
        return cmd

    #  设置点阵传感器
    def setMatrixSensor(self, data):
        cmd = self.mclib.SetSensorData(9, 1, data)
        if(cmd >= 1):
            print("点阵设置成功")
        else:
            print("点阵设置失败")
        return cmd

    #  读取声音传感器
    def readVoiceSensor(self):
        cmd = self.mclib.GetSensorData(4, 0)  # A0引脚 传感器ID 用4 模拟量
        return cmd

    #  读取巡线传感器 (ir2: cmd1   ir1: cmd2  黑色返回0 非黑色返回1)
    def readTrackingSensor(self):
        cmd1 = self.mclib.GetSensorData(15, 3)
        cmd2 = self.mclib.GetSensorData(15, 7)
        cmd = [cmd2, cmd1]
        return cmd

    #  读取遥控传感器
    def readRemoteSensor(self):
        self.mclib.SetSensorData(6, 1, [])
        cmd = self.mclib.ReadSensorData(17, 3)
       # 乐高Mini pro控制器后，读取遥控需要每次与传感器作交互，下位机不断上抛cmd=0的值，且灵感度降低
        if(cmd != -1 and cmd != 0):
            print("检测到指令：" + str(cmd))
        return cmd

    # 关闭遥控传感器
    def closeRemoteSensor(self):
        time.sleep(0.2)
        cmd = self.mclib.SetSensorData(6, 0, [])
        time.sleep(0.2)
        if(cmd >= 1):
            print("关闭遥控传感器设置成功")
        else:
            print("关闭遥控传感器设置失败")
        return cmd

    #  读取手势传感器
    def readGestureSensor(self):
        self.mclib.SetSensorData(12, 1, [])
        cmd = self.mclib.GetSensorData(30, 1)
        if(cmd != -1):
            print("检测到指令：" + str(cmd))
        return cmd

    # 关闭手势传感器

    def closeRemoteSensor(self):
        time.sleep(0.2)
        cmd = self.mclib.SetSensorData(12, 0, [])
        time.sleep(0.2)
        if(cmd >= 1):
            print("关闭手势传感器设置成功")
        else:
            print("关闭手势传感器设置失败")
        return cmd


######主函数######
# Testlib = BwSensorLib()
# # 声明控制库对象
# mlib = MiniBotControlLib("udp")
# # *************************
# #   1、连接设备
# #*************************#
# ip = "10.10.100.254"
# # ip = "127.0.0.1"
# r = mlib.Connect(ip, 8899)
# # 带超时的连接方式
# #r = mlib.Connect(ip,8899,2000)
# if(r > 0):
#     print("连接成功")
# else:
#     print("连接失败")
# rgbvalue = [[255,0,0],[0,255,0],[0,0,255],[0,0,0]]
# disdata1 = [[0x36,0x49,0x49,0x00,0x3E],[0x63,0x14,0x22,0x00,0x3E],[0x77,0x44,0x00,0x2A,0x14],
#          [0x41,0x36,0x00,0x1C,0x22],[0x22,0x77,0x22,0x1C,0x14],[0x20,0x12,0x20,0x08,0x1C],
#          [0x02,0x24,0x02,0x08,0x1C],[0x36,0x49,0x49,0x22,0x3E],[0x36,0x49,0x49,0x3E,0x22],
#          [0x00,0x00,0x00,0x00,0x00],[0x08,0x1C,0x2A,0x08,0x08],[0x08,0x08,0x2A,0x1C,0x08],
#          [0x08,0x10,0x3F,0x10,0x08],[0x08,0x04,0x7E,0x04,0x08],[0x00,0x00,0xFF,0x14,0x22],
#          [0x00,0x00,0xFC,0x14,0x22],[0x00,0x14,0x08,0x14,0x00],[0x22,0x14,0x08,0x14,0x22],
#          [0x1C,0x14,0x1C,0x14,0x12],[0x38,0x26,0x22,0x3E,0x02],[0x1E,0x12,0x14,0x12,0x1E]]
# fish1 = [1]
# fish0 = [0]
# while 1:
#    Testlib.readGestureSensor()#手势传感器
    # mlib.SetSensorData(6,0x01,fish1)
    # s1 = Testlib.readRemoteSensor() #遥控传感器
    # print ("返回值：",s1)


# 巡线传感器
# while 1:
#    s1 = Testlib.readGrayScaleSensor()
#    print(s1)
#    time.sleep(0.1)


# 手势传感器
# fish1 = [1]
# fish0 = [0]
# mlib.SetSensorData(12,1,fish1)
# while 1:
#    # s1 = Testlib.readGestureSensor()
#    # print(s1)

#    s2 = mlib.GetSensorData(12,1)
#    print(s2)
#    time.sleep(0.2)


# 声音传感器
# while 1:
#    s1 = Testlib.readVoiceSensor()
#    if(s1>400):
#       print("big")
    # time.sleep(0.5)
    # print(s1)


# # ##点阵传感器
# while 1:
#    for a in range (0,9):
#       print("1")
#       time.sleep(1)
#       Testlib.setLatticeSensor(disdata1[a])
#       print("点阵显示")
#       time.sleep(1)


# ###超声波传感器
# while 1:
#    s1 = Testlib.readUltrasonicSensor()
#    print(s1)
#    print("over")


# 颜色传感器
# while 1 :
#    result = Testlib.readColorSensor()
#    print(result)
#    time.sleep(0.2)
#    if(result == 1):
#       Testlib.setRGBLightSensor(rgbvalue[0])
#    elif(result == 2):
#       Testlib.setRGBLightSensor(rgbvalue[1])
#    elif(result == 3):
#       Testlib.setRGBLightSensor(rgbvalue[2])
#    else:
#       Testlib.setRGBLightSensor(rgbvalue[3])
#    print("over")


# RGB灯
# rgbvalue = [[255,0,0],[0,255,0],[0,0,255],[0,0,0]]
# print("准备测试")
# Testlib.setRGBLightSensor(rgbvalue[1])
# time.sleep(2)
# Testlib.setRGBLightSensor(rgbvalue[2])
# time.sleep(2)
# print("over")
